import rclpy
from rclpy.node import Node
from tf2_ros import StaticTransformBroadcaster
from geometry_msgs.msg import TransformStamped
from tf_transformations import quaternion_from_euler

class StaticTFPublisher(Node):
    def __init__(self):
        super().__init__('static_tf_broadcaster')
        
        # 创建静态TF广播器
        self.static_broadcaster = StaticTransformBroadcaster(self)
        
        # 发布所有静态TF变换
        self.publish_base_footprint_to_base_link()
        self.publish_base_link_to_laser()
        self.publish_base_link_to_imu_link()
        
        self.get_logger().info('静态TF发布节点已启动')

    def publish_base_footprint_to_base_link(self):
        """发布 base_footprint → base_link 的静态变换"""
        transform = TransformStamped()
        
        # 使用当前时间作为时间戳
        transform.header.stamp = self.get_clock().now().to_msg()
        transform.header.frame_id = 'base_footprint'  # 父坐标系
        transform.child_frame_id = 'base_link'  # 子坐标系
        
        # 设置平移 (base_link通常位于base_footprint上方)
        transform.transform.translation.x = 0.0
        transform.transform.translation.y = 0.0
        transform.transform.translation.z = 0.1  # 调整为实际高度
        
        # 设置旋转 (根据URDF中base_joint的rpy="0 0 0")
        q = quaternion_from_euler(0, 0, 0)
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]
        
        # 发布静态TF
        self.static_broadcaster.sendTransform(transform)
        self.get_logger().info('发布了 base_footprint → base_link 的静态变换')

    def publish_base_link_to_laser(self):
        """发布 base_link → laser_link 的静态变换"""
        transform = TransformStamped()
        
        # 使用当前时间作为时间戳
        transform.header.stamp = self.get_clock().now().to_msg()
        transform.header.frame_id = 'base_link'  # 父坐标系
        transform.child_frame_id = 'laser_link'  # 子坐标系
        
        # 设置平移 (激光雷达通常安装在机器人上方)
        transform.transform.translation.x = 0.0
        transform.transform.translation.y = 0.0
        transform.transform.translation.z = 0.2  # 调整为实际高度
        
        # 设置旋转 (如果激光雷达朝前安装，通常不需要旋转)
        q = quaternion_from_euler(0, 0, 0)
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]
        
        # 发布静态TF
        self.static_broadcaster.sendTransform(transform)
        self.get_logger().info('发布了 base_link → laser_link 的静态变换')


    def publish_base_link_to_imu_link(self):
        """发布 base_footprint → base_link 的静态变换"""
        transform = TransformStamped()
        
        # 使用当前时间作为时间戳
        transform.header.stamp = self.get_clock().now().to_msg()
        transform.header.frame_id = 'base_link'  # 父坐标系
        transform.child_frame_id = 'imu_link'  # 子坐标系
        
        # 设置平移 (base_link通常位于base_footprint上方)
        transform.transform.translation.x = 0.0
        transform.transform.translation.y = 0.0
        transform.transform.translation.z = 0.15  # 调整为实际高度
        
        # 设置旋转 (根据URDF中base_joint的rpy="0 0 0")
        q = quaternion_from_euler(0, 0, 0)
        transform.transform.rotation.x = q[0]
        transform.transform.rotation.y = q[1]
        transform.transform.rotation.z = q[2]
        transform.transform.rotation.w = q[3]
        
        # 发布静态TF
        self.static_broadcaster.sendTransform(transform)
        self.get_logger().info('发布了 base_link → imu_link 的静态变换')


def main(args=None):
    rclpy.init(args=args)
    node = StaticTFPublisher()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
